#include "linear_steady_state.h"
namespace auto_ros
{
namespace control
{
LinearSteadyState::LinearSteadyState(std::shared_ptr<ContinuousLinearSys> linear_sys_ptr)
{
	linear_sys_ptr_ = linear_sys_ptr;
	x_ = qp_.addVariable("x", linear_sys_ptr_->state_dim());
	u_ = qp_.addVariable("u", linear_sys_ptr_->control_dim());
	s_ = qp_.addVariable("s", linear_sys_ptr_->state_dim());
	distur_vec_ = Eigen::VectorXd::Zero(linear_sys_ptr_->distur_dim());
	out_vec_ = Eigen::VectorXd::Zero(linear_sys_ptr_->out_dim());
	//slack state equation
	qp_.addConstraint(cvx::equalTo(cvx::dynpar(linear_sys_ptr_->distur_matrix_) * cvx::dynpar(distur_vec_) +
									   cvx::dynpar(linear_sys_ptr_->state_matrix_) * x_ + cvx::dynpar(linear_sys_ptr_->control_matrix_) * u_,
								   s_));
	// non-offset output constriants
	qp_.addConstraint(cvx::equalTo(cvx::par(out_vec_), cvx::dynpar(linear_sys_ptr_->out_state_matrix_) * x_));
	qp_.addCostTerm(s_.cwiseAbs2().sum());
}
bool LinearSteadyState::add_slack_states(std::vector<int> state_index_vec)
{
	for (int index = 0; index < linear_sys_ptr_->state_dim(); index++)
	{
		auto it = std::find(state_index_vec.begin(), state_index_vec.end(), index);
		if (it == state_index_vec.end())
		{
			qp_.addConstraint(cvx::equalTo(s_(index), 0));
		}
	}
	return true;
}
bool LinearSteadyState::set_distur_vec(Eigen::VectorXd distur_vec)
{
	if (distur_vec.rows() == linear_sys_ptr_->distur_dim())
	{
		distur_vec_ = distur_vec;
		return true;
	}
	else
	{
		std::cerr << "\033[31m LinearSteadyState::set_ditur_vec failed dim is not right \033[0m" << std::endl;
		return false;
	}
}
bool LinearSteadyState::calc_steady_state(const Eigen::VectorXd ditur_vec,
										  Eigen::VectorXd &steady_state,
										  Eigen::VectorXd &steady_ctr,
										  Eigen::VectorXd &slack_vec)
{
	if (set_distur_vec(ditur_vec) == false)
	{
		std::cerr << "\033[31m LinearSteadyState::calc_steady_state failed, set_distur_vec failed \033[0m" << std::endl;
	}
	if (solver_ptr_->solve(false) == false)
	{
		std::cerr << "\033[31m LinearSteadyState::calc_steady_state failed, solve failed \033[0m" << std::endl;
	}
	steady_state = cvx::eval(x_);
	steady_ctr = cvx::eval(u_);
	slack_vec = cvx::eval(s_);
}
bool LinearSteadyState::init_solver_ptr()
{
	solver_ptr_ = std::make_shared<cvx::osqp::OSQPSolver>(qp_);
}
} // namespace control
} // namespace auto_ros